using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Graphics;
//using Poly2Tri;
using GameStateManagement;
using Microsoft.Xna.Framework.Content;
using PolyDecomp;

namespace GameStateManagementSample.Classes
{
    public class DirectionPoint
    {
        public DirectionPoint(Vector2 position, float rotation)
        {
            this.Position = position;
            this.Rotation = rotation;
        }
        public Vector2 Position { get; set; }
        public float Rotation { get; set; }
    }

    public class BezierSegment
    {
        public BezierSegment(Vector2 a, Vector2 b, Vector2 c)
        {
            this.p1 = a;
            this.p2 = b;
            this.p3 = c;
        }
        public Vector2 p1 { get; set; }
        public Vector2 p2 { get; set; }
        public Vector2 p3 { get; set; }

    }

    class SmoothPath
    {


        Texture2D Dash;
        Texture2D point;
        public SmoothPath(ContentManager content)
        {
            Dash = content.Load<Texture2D>("dash");
            point = content.Load<Texture2D>("point");
            //  effect = eff;

        }
        public List<Vector2> pathPoints = new List<Vector2>();

        public void AddPoint(Vector2 P)
        {
            pathPoints.Add(P);

        }
        //private static void EnsureInitialization(GraphicsDevice g)
        //{
        //    var device = g;
        //    var basicEffect = new BasicEffect(device);
        //    basicEffect.Projection = Matrix.CreateOrthographicOffCenter
        //        (0, device.Viewport.Width,
        //        device.Viewport.Height, 0,
        //        0, 1);
        //    basicEffect.CurrentTechnique.Passes[0].Apply();
        //    foreach (EffectPass pass in basicEffect.CurrentTechnique.Passes)
        //    {
        //        pass.Apply();

        //    }
        //}
        public void Draw(SpriteBatch spriteBatch)
        {
            float offset = 0;
            // EnsureInitialization(spriteBatch.GraphicsDevice);
            //var vertices = new VertexPositionColor[3];

            //vertices[0].Position = new Vector3(100f, 100f, 0f);
            //vertices[0].Color = Color.Red;
            //vertices[1].Position = new Vector3(0, 100f, 0f);
            //vertices[1].Color = Color.Red;
            //vertices[2].Position = new Vector3(200f, 200f, 0f);
            //vertices[2].Color = Color.Red;

            //spriteBatch.GraphicsDevice.DrawUserPrimitives(PrimitiveType.TriangleList, vertices, 0, 1, VertexPositionColor.VertexDeclaration);

            if (pathPoints.Count <= 1) return;
            if (pathPoints.Count == 2)
            {

                LineBatchTester.LineBatch.DrawLine(spriteBatch, Color.Red, pathPoints[0], pathPoints[1]);
            }
            else
            {


                offset = DrawBezierSegmentDashes(pathPoints[0], pathPoints[1], GeometryHelper.LineMiddle(pathPoints[1], pathPoints[2]), spriteBatch, offset);
                for (int i = 3; i < pathPoints.Count - 1; i++)
                {
                    offset = DrawBezierSegmentDashes(GeometryHelper.LineMiddle(pathPoints[i - 2], pathPoints[i - 1]), pathPoints[i - 1], GeometryHelper.LineMiddle(pathPoints[i - 1], pathPoints[i]), spriteBatch, offset);

                }
                offset = DrawBezierSegmentDashes(GeometryHelper.LineMiddle(pathPoints[pathPoints.Count - 3], pathPoints[pathPoints.Count - 2]), pathPoints[pathPoints.Count - 2], pathPoints[pathPoints.Count - 1], spriteBatch, offset);

            }
            foreach (var vect in pathPoints)
             {
                 spriteBatch.Draw(point, vect - new Vector2(point.Width / 2, point.Height / 2), Color.White);
              
             }
            //List<Vector2> pol = ToPolygon((int)GetPathLength() / 30);
            //if (pol != null)
            //{
            //    Vector2 last = pol.Last();
            //    foreach (var vect in pol)
            //    {
            //        LineBatchTester.LineBatch.DrawLine(spriteBatch, Color.Red, vect, last);
            //        last = vect;
            //    }
            //}

            //if (pathPoints.Count > 2)
            //{
            //    try
            //    {
            //        poly = ToPolygonTriangulated((int)GetPathLength() / 30);

            //        var tri = new VertexPositionColor[poly.Triangles.Count * 3];
            //        for (var i = 0; i < poly.Triangles.Count; i++)
            //        {
            //            Color color = Color.Goldenrod;
            //            tri[i * 3] = new VertexPositionColor(new Vector3(poly.Triangles[i].Points[0].Xf,
            //                                                       poly.Triangles[i].Points[0].Yf,
            //                                                       0), Color.Yellow);
            //            tri[i * 3 + 1] = new VertexPositionColor(new Vector3(poly.Triangles[i].Points[1].Xf,
            //                                                             poly.Triangles[i].Points[1].Yf,
            //                                                             0), Color.Red);
            //            tri[i * 3 + 2] = new VertexPositionColor(new Vector3(poly.Triangles[i].Points[2].Xf,
            //                                                             poly.Triangles[i].Points[2].Yf,
            //                                                             0), Color.Brown);
            //            //LineBatchTester.LineBatch.DrawLine(spriteBatch, Color.Green, (Vector2)poly.Triangles[i].Points[0], (Vector2)poly.Triangles[i].Points[1]);
            //            //LineBatchTester.LineBatch.DrawLine(spriteBatch, Color.Green, (Vector2)poly.Triangles[i].Points[1], (Vector2)poly.Triangles[i].Points[2]);
            //            //LineBatchTester.LineBatch.DrawLine(spriteBatch, Color.Green, (Vector2)poly.Triangles[i].Points[0], (Vector2)poly.Triangles[i].Points[2]);
            //        }

            //        List<VertexPositionColor> vertices = new List<VertexPositionColor>();
            //        foreach (var pnt in poly.Points) 
            //        {
            //            vertices.Add(new VertexPositionColor(new Vector3(pnt.Xf, pnt.Yf, 0), Color.Goldenrod));
            //        }
            //        //VertexPositionColor last = tri.Last();
            //        //    foreach (var vpc in tri)
            //        //    {
            //        //        LineBatchTester.LineBatch.DrawLine(spriteBatch, Color.Green, new Vector2(vpc.Position.X, vpc.Position.Y), new Vector2(last.Position.X, last.Position.Y));
            //        //        last = vpc;
            //        //    }


            //        //List<VertexPositionColor> vertices = new List<VertexPositionColor>();
            //        //foreach (var triangle in poly.Triangles)
            //        //{
            //        //    var vpc = new VertexPositionColor();
            //        //    vpc.Color = Color.Goldenrod;
            //        //    vpc.Position = new Vector3(triangle.Points[0].Xf,
            //        //                                   triangle.Points[0].Yf,
            //        //                                   0);
            //        //    vertices.Add(vpc);
            //        //    vpc = new VertexPositionColor();
            //        //    vpc.Color = Color.Goldenrod;
            //        //    vpc.Position = new Vector3(triangle.Points[1].Xf,
            //        //                                   triangle.Points[1].Yf,
            //        //                                   0);
            //        //    vertices.Add(vpc);
            //        //    vpc = new VertexPositionColor();
            //        //    vpc.Color = Color.Goldenrod;
            //        //    vpc.Position = new Vector3(triangle.Points[2].Xf,
            //        //                                   triangle.Points[2].Yf,
            //        //                                   0);
            //        //    vertices.Add(vpc);
            //        //}

            //        //GraphicsDevice.SetVertexBuffer(vertexBuffer);

            //        var gd = spriteBatch.GraphicsDevice;
            //        BasicEffect effect = new BasicEffect(gd);

            //        effect.Projection = Matrix.CreateOrthographicOffCenter
            //            (0, gd.Viewport.Width,
            //            gd.Viewport.Height, 0,
            //            0, 1);
            //        effect.VertexColorEnabled = true;
            //        //effect.CurrentTechnique.Passes[0].Apply();
            //        foreach (EffectPass pass in effect.CurrentTechnique.Passes)
            //        {
            //            pass.Apply();
            //            spriteBatch.GraphicsDevice.DrawUserPrimitives<VertexPositionColor>(PrimitiveType.TriangleList, tri, 0, poly.Triangles.Count , VertexPositionColor.VertexDeclaration);
            //            //spriteBatch.GraphicsDevice.DrawUserPrimitives<VertexPositionColor>(PrimitiveType.TriangleList, vertices.ToArray(), 0, poly.Triangles.Count, VertexPositionColor.VertexDeclaration);


            //        }
            //        effect.VertexColorEnabled = false;

            //        //spriteBatch.GraphicsDevice.DrawUserPrimitives<VertexPositionColor>(PrimitiveType.LineStrip, tri, 0, poly.Triangles.Count - 1, VertexPositionColor.VertexDeclaration);
            //    }
            //    catch { }
            //}
        }
        //List<Vector2> poly;

        int segments = 10;
        Color color = Color.Red;
        public void DrawBezierSegment(Vector2 p1, Vector2 p2, Vector2 p3, SpriteBatch spriteBatch)
        {
            CurveMap<BezierSegment> curveMap = new CurveMap<BezierSegment>(10);
            var bs = new BezierSegment(p1, p2, p3);
            curveMap.set(bs);

            int verticesCount = segments;
            VertexPositionColor[] vertices = new VertexPositionColor[verticesCount];
            Vector2 pnt, prevpnt = Vector2.Zero;
            int i = 0;
            float amount = 0f;
            for (int j = 0; j <= segments - 1; j++)
            {
                //float x = (float)Math.Pow(1 - amount, 2) * p3.X + 2.0f * (1 - amount) * amount * p2.X + amount * amount * p1.X;
                //float y = (float)Math.Pow(1 - amount, 2) * p3.Y + 2.0f * (1 - amount) * amount * p2.Y + amount * amount * p1.Y;

                pnt = curveMap.sample(amount);
                //pnt = BezierHelper.GetQuadraticBezierPointAt(amount, p1, p2, p3);

                //vertices[i * segments + j] = new VertexPositionColor();
                //vertices[i * segments + j].Position = new Vector3(x, y, 0);
                //vertices[i * segments + j].Color = color;
                //pnt = new Vector2(x, y);
                if (j > 0) { LineBatchTester.LineBatch.DrawLine(spriteBatch, Color.Red, pnt, prevpnt); }
                prevpnt = pnt;
                amount += 1f / (segments - 1);
            }

            //g.GraphicsDevice.DrawUserPrimitives<VertexPositionColor>(PrimitiveType.LineStrip, vertices, 0, verticesCount - 1);


        }

        public float DrawBezierSegmentDashes(Vector2 p1, Vector2 p2, Vector2 p3, SpriteBatch spriteBatch, float offset)
        {

            CurveMap<BezierSegment> curveMap = new CurveMap<BezierSegment>(100);
            var bs = new BezierSegment(p1, p2, p3);
            curveMap.set(bs);

            float len = BezierHelper.GetQuadraticBezierLength(p1, p2, p3);
            float step = Dash.Bounds.Width * 1.5f;

            Vector2 pnt;
            float rot;
            float amount = offset;
            float t = curveMap.map(amount / len);
            //len -=  offset;
            // float steps = len / step;
            rot = BezierHelper.QuadraticBezierAngle(0f, p1, p2, p3);
            pnt = BezierHelper.GetQuadraticBezierPointAt(0f, p1, p2, p3);
            //spriteBatch.Draw(Dash, pnt, Dash.Bounds, Color.White, rot, Vector2.Zero, Vector2.One, SpriteEffects.None, 0);
            LineBatchTester.LineBatch.DrawLine(spriteBatch, Color.Red, pnt, new Vector2(pnt.X + 40 * (float)Math.Sin(rot), pnt.Y - 40 * (float)Math.Cos(rot)));
            rot = BezierHelper.QuadraticBezierAngle(1f, p1, p2, p3);
            pnt = BezierHelper.GetQuadraticBezierPointAt(1f, p1, p2, p3);
            //spriteBatch.Draw(Dash, pnt, Dash.Bounds, Color.White, rot, Vector2.Zero, Vector2.One, SpriteEffects.None, 0);
            LineBatchTester.LineBatch.DrawLine(spriteBatch, Color.Red, pnt, new Vector2(pnt.X + 40 * (float)Math.Sin(rot), pnt.Y - 40 * (float)Math.Cos(rot)));

            while (amount < len)
            {
                // pnt = curveMap.sample(amount);
                rot = BezierHelper.QuadraticBezierAngle(t, p1, p2, p3);
                pnt = BezierHelper.GetQuadraticBezierPointAt(t, p1, p2, p3);
                spriteBatch.Draw(Dash, pnt, Dash.Bounds, Color.White, rot, new Vector2(Dash.Bounds.Width / 2, Dash.Bounds.Height / 2), Vector2.One, SpriteEffects.None, 0);
                //spriteBatch.Draw(Dash, pnt, Dash.Bounds, Color.White, rot, Vector2.Zero, Vector2.One, SpriteEffects.None, 0);

                amount += step;
                t = curveMap.map(amount / len);
            }
            return amount - len;
            //int verticesCount = segments;
            //VertexPositionColor[] vertices = new VertexPositionColor[verticesCount];
            //Vector2 pnt, prevpnt = Vector2.Zero;
            //int i = 0;
            //float amount = 0f;
            //for (int j = 0; j <= segments - 1; j++)
            //{
            //    float x = (float)Math.Pow(1 - amount, 2) * p3.X + 2.0f * (1 - amount) * amount * p2.X + amount * amount * p1.X;
            //    float y = (float)Math.Pow(1 - amount, 2) * p3.Y + 2.0f * (1 - amount) * amount * p2.Y + amount * amount * p1.Y;

            //    pnt = new Vector2(x, y);
            //    if (j > 0) { LineBatchTester.LineBatch.DrawLine(spriteBatch, Color.Red, pnt, prevpnt); }
            //    prevpnt = pnt;
            //    amount += 1f / (segments - 1);
            //}
        }

        public float GetPathLength()
        {
            float len = 0f;
            if (pathPoints.Count == 1 || pathPoints.Count == 0) return 0;
            if (pathPoints.Count == 2)
            {
                Vector2.Distance(pathPoints[0], pathPoints[1]);
            }
            else if (pathPoints.Count == 3)
            {
                len += BezierHelper.GetQuadraticBezierLength(pathPoints[0], pathPoints[1], pathPoints[2]);
            }
            else
            {

                len += BezierHelper.GetQuadraticBezierLength(pathPoints[0], pathPoints[1], GeometryHelper.LineMiddle(pathPoints[1], pathPoints[2]));
                for (int i = 3; i < pathPoints.Count - 1; i++)
                {
                    len += BezierHelper.GetQuadraticBezierLength(GeometryHelper.LineMiddle(pathPoints[i - 2], pathPoints[i - 1]), pathPoints[i - 1], GeometryHelper.LineMiddle(pathPoints[i - 1], pathPoints[i]));

                }
                len += BezierHelper.GetQuadraticBezierLength(GeometryHelper.LineMiddle(pathPoints[pathPoints.Count - 3], pathPoints[pathPoints.Count - 2]), pathPoints[pathPoints.Count - 2], pathPoints[pathPoints.Count - 1]);

            }

            return len;
        }

        public int GetPathSegmentAtLength(float d)
        {
            float len = 0f;
            if (pathPoints.Count == 3)
            {
                len += BezierHelper.GetQuadraticBezierLength(pathPoints[0], pathPoints[1], pathPoints[2]);
                if (d <= len) return 2;

            }
            else
            {
                len += BezierHelper.GetQuadraticBezierLength(pathPoints[0], pathPoints[1], GeometryHelper.LineMiddle(pathPoints[1], pathPoints[2]));
                if (d <= len) return 2;
                for (int i = 3; i < pathPoints.Count - 1; i++)
                {
                    len += BezierHelper.GetQuadraticBezierLength(GeometryHelper.LineMiddle(pathPoints[i - 2], pathPoints[i - 1]), pathPoints[i - 1], GeometryHelper.LineMiddle(pathPoints[i - 1], pathPoints[i]));
                    if (d <= len) return i;
                }
                len += BezierHelper.GetQuadraticBezierLength(GeometryHelper.LineMiddle(pathPoints[pathPoints.Count - 3], pathPoints[pathPoints.Count - 2]), pathPoints[pathPoints.Count - 2], pathPoints[pathPoints.Count - 1]);
                if (d <= len) return pathPoints.Count - 1;
            }

            return -1;
        }

        public DirectionPoint GetPositionAtLength(float d)
        {
            DirectionPoint dp;
            if (pathPoints.Count <= 1) return null;
            float segmentLength = 0f;
            float len = 0f;
            if (pathPoints.Count == 2)
            {
                segmentLength = Vector2.Distance(pathPoints[0], pathPoints[1]);
                if (d <= len + segmentLength)
                {
                    Vector2 vect = pathPoints[0] + pathPoints[1];
                    return new DirectionPoint(GeometryHelper.GetLinePointAt((d - len) / segmentLength, pathPoints[0], pathPoints[1]), (float)Math.Atan2(vect.Y, vect.X));
                }
                len += segmentLength;
            }
            else if (pathPoints.Count == 3)
            {
                dp = GetSegmentDirectionPointAt(pathPoints[0], pathPoints[1], pathPoints[2], d, ref len);
                if (dp != null) return dp;
                //segmentLength = BezierHelper.GetQuadraticBezierLength(pathPoints[0], pathPoints[1], pathPoints[2]);
                //if (d <= len + segmentLength)
                //    return new DirectionPoint(
                //        BezierHelper.GetQuadraticBezierPointAt((d - len) / segmentLength, pathPoints[0], pathPoints[1], pathPoints[2]),
                //       BezierHelper.QuadraticBezierAngle((d - len) / segmentLength, pathPoints[0], pathPoints[1], pathPoints[2])
                //    );
                //len += segmentLength;
            }
            else
            {
                dp = GetSegmentDirectionPointAt(pathPoints[0], pathPoints[1], GeometryHelper.LineMiddle(pathPoints[1], pathPoints[2]), d, ref len);
                if (dp != null) return dp;
                //segmentLength = BezierHelper.GetQuadraticBezierLength(pathPoints[0], pathPoints[1], GeometryHelper.LineMiddle(pathPoints[1], pathPoints[2]));
                //if (d <= len + segmentLength)
                //    return new DirectionPoint(
                //    BezierHelper.GetQuadraticBezierPointAt((d - len) / segmentLength, pathPoints[0], pathPoints[1], GeometryHelper.LineMiddle(pathPoints[1], pathPoints[2])),
                //    BezierHelper.QuadraticBezierAngle((d - len) / segmentLength, pathPoints[0], pathPoints[1], GeometryHelper.LineMiddle(pathPoints[1], pathPoints[2]))
                //    );
                //len += segmentLength;

                for (int i = 3; i < pathPoints.Count - 1; i++)
                {
                    dp = GetSegmentDirectionPointAt(GeometryHelper.LineMiddle(pathPoints[i - 2], pathPoints[i - 1]), pathPoints[i - 1], GeometryHelper.LineMiddle(pathPoints[i - 1], pathPoints[i]), d, ref len);
                    if (dp != null) return dp;

                    // segmentLength = BezierHelper.GetQuadraticBezierLength(GeometryHelper.LineMiddle(pathPoints[i - 2], pathPoints[i - 1]), pathPoints[i - 1], GeometryHelper.LineMiddle(pathPoints[i - 1], pathPoints[i]));
                    // if (d <= len + segmentLength) return new DirectionPoint(
                    //BezierHelper.GetQuadraticBezierPointAt((d - len) / segmentLength, GeometryHelper.LineMiddle(pathPoints[i - 2], pathPoints[i - 1]), pathPoints[i - 1], GeometryHelper.LineMiddle(pathPoints[i - 1], pathPoints[i])),
                    //      BezierHelper.QuadraticBezierAngle((d - len) / segmentLength, GeometryHelper.LineMiddle(pathPoints[i - 2], pathPoints[i - 1]), pathPoints[i - 1], GeometryHelper.LineMiddle(pathPoints[i - 1], pathPoints[i]))
                    // );
                    // len += segmentLength;
                }
                dp = GetSegmentDirectionPointAt(GeometryHelper.LineMiddle(pathPoints[pathPoints.Count - 3], pathPoints[pathPoints.Count - 2]), pathPoints[pathPoints.Count - 2], pathPoints[pathPoints.Count - 1], d, ref len);
                if (dp != null) return dp;
                //segmentLength = BezierHelper.GetQuadraticBezierLength(GeometryHelper.LineMiddle(pathPoints[pathPoints.Count - 3], pathPoints[pathPoints.Count - 2]), pathPoints[pathPoints.Count - 2], pathPoints[pathPoints.Count - 1]);
                //if (d <= len + segmentLength)
                //    return new DirectionPoint(
                //    BezierHelper.GetQuadraticBezierPointAt((d - len) / segmentLength, GeometryHelper.LineMiddle(pathPoints[pathPoints.Count - 3], pathPoints[pathPoints.Count - 2]), pathPoints[pathPoints.Count - 2], pathPoints[pathPoints.Count - 1]),
                //       BezierHelper.QuadraticBezierAngle((d - len) / segmentLength, GeometryHelper.LineMiddle(pathPoints[pathPoints.Count - 3], pathPoints[pathPoints.Count - 2]), pathPoints[pathPoints.Count - 2], pathPoints[pathPoints.Count - 1])
                //    );
                //len += segmentLength;
            }

            return new DirectionPoint(-Vector2.One, 0f);
        }

        private DirectionPoint GetSegmentDirectionPointAt(Vector2 p1, Vector2 p2, Vector2 p3, float d, ref float len)
        {

            CurveMap<BezierSegment> curveMap = new CurveMap<BezierSegment>(100);
            var bs = new BezierSegment(p1, p2, p3);
            curveMap.set(bs);

            var segmentLength = BezierHelper.GetQuadraticBezierLength(p1, p2, p3);
            if (d <= len + segmentLength)
            {
                var t = curveMap.map((d - len) / segmentLength);
                //var t = (d - len) / segmentLength;
                return new DirectionPoint(
                BezierHelper.GetQuadraticBezierPointAt(t, p1, p2, p3),
                   BezierHelper.QuadraticBezierAngle(t, p1, p2, p3)
                );
            }
            len += segmentLength;
            return null;
        }

        public DirectionPoint GetPositionAtLength_old(float d)
        {

            if (pathPoints.Count <= 1) return null;
            float segmentLength = 0f;
            float len = 0f;
            if (pathPoints.Count == 2)
            {
                segmentLength = Vector2.Distance(pathPoints[0], pathPoints[1]);
                if (d <= len + segmentLength)
                {
                    Vector2 vect = pathPoints[0] + pathPoints[1];
                    return new DirectionPoint(GeometryHelper.GetLinePointAt((d - len) / segmentLength, pathPoints[0], pathPoints[1]), (float)Math.Atan2(vect.Y, vect.X));
                }
                len += segmentLength;
            }
            else if (pathPoints.Count == 3)
            {
                segmentLength = BezierHelper.GetQuadraticBezierLength(pathPoints[0], pathPoints[1], pathPoints[2]);
                if (d <= len + segmentLength)
                    return new DirectionPoint(
                        BezierHelper.GetQuadraticBezierPointAt((d - len) / segmentLength, pathPoints[0], pathPoints[1], pathPoints[2]),
                       BezierHelper.QuadraticBezierAngle((d - len) / segmentLength, pathPoints[0], pathPoints[1], pathPoints[2])
                    );
                len += segmentLength;
            }
            else
            {
                segmentLength = BezierHelper.GetQuadraticBezierLength(pathPoints[0], pathPoints[1], GeometryHelper.LineMiddle(pathPoints[1], pathPoints[2]));
                if (d <= len + segmentLength)
                    return new DirectionPoint(
                    BezierHelper.GetQuadraticBezierPointAt((d - len) / segmentLength, pathPoints[0], pathPoints[1], GeometryHelper.LineMiddle(pathPoints[1], pathPoints[2])),
                    BezierHelper.QuadraticBezierAngle((d - len) / segmentLength, pathPoints[0], pathPoints[1], GeometryHelper.LineMiddle(pathPoints[1], pathPoints[2]))
                    );
                len += segmentLength;

                for (int i = 3; i < pathPoints.Count - 1; i++)
                {
                    segmentLength = BezierHelper.GetQuadraticBezierLength(GeometryHelper.LineMiddle(pathPoints[i - 2], pathPoints[i - 1]), pathPoints[i - 1], GeometryHelper.LineMiddle(pathPoints[i - 1], pathPoints[i]));
                    if (d <= len + segmentLength) return new DirectionPoint(
                   BezierHelper.GetQuadraticBezierPointAt((d - len) / segmentLength, GeometryHelper.LineMiddle(pathPoints[i - 2], pathPoints[i - 1]), pathPoints[i - 1], GeometryHelper.LineMiddle(pathPoints[i - 1], pathPoints[i])),
                         BezierHelper.QuadraticBezierAngle((d - len) / segmentLength, GeometryHelper.LineMiddle(pathPoints[i - 2], pathPoints[i - 1]), pathPoints[i - 1], GeometryHelper.LineMiddle(pathPoints[i - 1], pathPoints[i]))
                    );
                    len += segmentLength;
                }
                segmentLength = BezierHelper.GetQuadraticBezierLength(GeometryHelper.LineMiddle(pathPoints[pathPoints.Count - 3], pathPoints[pathPoints.Count - 2]), pathPoints[pathPoints.Count - 2], pathPoints[pathPoints.Count - 1]);
                if (d <= len + segmentLength)
                    return new DirectionPoint(
                    BezierHelper.GetQuadraticBezierPointAt((d - len) / segmentLength, GeometryHelper.LineMiddle(pathPoints[pathPoints.Count - 3], pathPoints[pathPoints.Count - 2]), pathPoints[pathPoints.Count - 2], pathPoints[pathPoints.Count - 1]),
                       BezierHelper.QuadraticBezierAngle((d - len) / segmentLength, GeometryHelper.LineMiddle(pathPoints[pathPoints.Count - 3], pathPoints[pathPoints.Count - 2]), pathPoints[pathPoints.Count - 2], pathPoints[pathPoints.Count - 1])
                    );
                len += segmentLength;
            }

            return new DirectionPoint(-Vector2.One, 0f);
        }

        public List<Vector2> ToPolygon(int samples)
        {
            if (pathPoints.Count <= 2) return null;

            float totalLen = GetPathLength();
            float step = totalLen / samples;

            float len, currentLen = 0;

            float d = 0;

            List<Vector2> points = new List<Vector2>();

            if (pathPoints.Count == 3)
            {

                len = BezierHelper.GetQuadraticBezierLength(pathPoints[0], pathPoints[1], pathPoints[2]);
                while (d < currentLen + len)
                {
                    float t = (d - currentLen) / len;
                    points.Add(BezierHelper.GetQuadraticBezierPointAt(t, pathPoints[0], pathPoints[1], pathPoints[2]));

                    d += step;
                }
                currentLen += len;


            }
            else
            {

                len = BezierHelper.GetQuadraticBezierLength(pathPoints[0], pathPoints[1], GeometryHelper.LineMiddle(pathPoints[1], pathPoints[2]));
                while (d < currentLen + len)
                {
                    float t = (d - currentLen) / len;
                    points.Add(BezierHelper.GetQuadraticBezierPointAt(t, pathPoints[0], pathPoints[1], GeometryHelper.LineMiddle(pathPoints[1], pathPoints[2])));

                    d += step;
                }
                currentLen += len;

                for (int i = 3; i < pathPoints.Count - 1; i++)
                {

                    len = BezierHelper.GetQuadraticBezierLength(GeometryHelper.LineMiddle(pathPoints[i - 2], pathPoints[i - 1]), pathPoints[i - 1], GeometryHelper.LineMiddle(pathPoints[i - 1], pathPoints[i]));
                    while (d < currentLen + len)
                    {
                        float t = (d - currentLen) / len;
                        points.Add(BezierHelper.GetQuadraticBezierPointAt(t, GeometryHelper.LineMiddle(pathPoints[i - 2], pathPoints[i - 1]), pathPoints[i - 1], GeometryHelper.LineMiddle(pathPoints[i - 1], pathPoints[i])));

                        d += step;
                    }
                    currentLen += len;
                }

                len = BezierHelper.GetQuadraticBezierLength(GeometryHelper.LineMiddle(pathPoints[pathPoints.Count - 3], pathPoints[pathPoints.Count - 2]), pathPoints[pathPoints.Count - 2], pathPoints[pathPoints.Count - 1]);
                while (d < currentLen + len)
                {
                    float t = (d - currentLen) / len;
                    points.Add(BezierHelper.GetQuadraticBezierPointAt(t, GeometryHelper.LineMiddle(pathPoints[pathPoints.Count - 3], pathPoints[pathPoints.Count - 2]), pathPoints[pathPoints.Count - 2], pathPoints[pathPoints.Count - 1]));

                    d += step;
                }
                currentLen += len;
            }
            var dist = Vector2.Distance(pathPoints.Last(), points.Last());
            if (dist > 0 &&dist< 5) 
            {
                points[points.Count-1] = pathPoints.Last();
            }
            else if (dist >5)
            {
                points.Add(pathPoints.Last());
            }
            return points;
        }



        public List<Triangle> ToPolygonTriangulated(int samples)
        {

            if (pathPoints.Count <= 2) return null;

            float totalLen = GetPathLength();
            float step = totalLen / samples;

            float len, currentLen = 0;

            float d = 0;

            List<Vector2> points = new List<Vector2>();

            if (pathPoints.Count == 3)
            {

                len = BezierHelper.GetQuadraticBezierLength(pathPoints[0], pathPoints[1], pathPoints[2]);
                while (d < currentLen + len)
                {
                    float t = (d - currentLen) / len;
                    points.Add(BezierHelper.GetQuadraticBezierPointAt(t, pathPoints[0], pathPoints[1], pathPoints[2]));

                    d += step;
                }
                currentLen += len;


            }
            else
            {

                len = BezierHelper.GetQuadraticBezierLength(pathPoints[0], pathPoints[1], GeometryHelper.LineMiddle(pathPoints[1], pathPoints[2]));
                while (d < currentLen + len)
                {
                    float t = (d - currentLen) / len;
                    points.Add(BezierHelper.GetQuadraticBezierPointAt(t, pathPoints[0], pathPoints[1], GeometryHelper.LineMiddle(pathPoints[1], pathPoints[2])));

                    d += step;
                }
                currentLen += len;

                for (int i = 3; i < pathPoints.Count - 1; i++)
                {

                    len = BezierHelper.GetQuadraticBezierLength(GeometryHelper.LineMiddle(pathPoints[i - 2], pathPoints[i - 1]), pathPoints[i - 1], GeometryHelper.LineMiddle(pathPoints[i - 1], pathPoints[i]));
                    while (d < currentLen + len)
                    {
                        float t = (d - currentLen) / len;
                        points.Add(BezierHelper.GetQuadraticBezierPointAt(t, GeometryHelper.LineMiddle(pathPoints[i - 2], pathPoints[i - 1]), pathPoints[i - 1], GeometryHelper.LineMiddle(pathPoints[i - 1], pathPoints[i])));

                        d += step;
                    }
                    currentLen += len;
                }

                len = BezierHelper.GetQuadraticBezierLength(GeometryHelper.LineMiddle(pathPoints[pathPoints.Count - 3], pathPoints[pathPoints.Count - 2]), pathPoints[pathPoints.Count - 2], pathPoints[pathPoints.Count - 1]);
                while (d < currentLen + len)
                {
                    float t = (d - currentLen) / len;
                    points.Add(BezierHelper.GetQuadraticBezierPointAt(t, GeometryHelper.LineMiddle(pathPoints[pathPoints.Count - 3], pathPoints[pathPoints.Count - 2]), pathPoints[pathPoints.Count - 2], pathPoints[pathPoints.Count - 1]));

                    d += step;
                }
                currentLen += len;
            }
           var p = Triangulation.triangulatePolygon(points);
            //var p = new Poly2Tri.Polygon(points);
            //P2T.Triangulate(p);
            return p.ToList();
        }
    }
}
